#include <vector>

enum queued_canon_type
{
    QSTRAIGHT_TRAVERSE,
    QSTRAIGHT_FEED,
    QARC_FEED,
    QSET_FEED_RATE,
    QDWELL,
    QSETTOOL,
    QSET_FEED_MODE,
    QCOMMENT,
    QM_USER_COMMAND,
    QSETUSERFRAME,
    QENABLEADMIT,
    QSETPAYLOAD,
    QSETCLSNLEVEL,
    QSETADMITCTRLCONFIG,
    QSETCOMPLIANTCON,
    QSETCOMPLIANTTYPE,
    QSETVELCOMPLIANTLEVEL,
    QSETFORCECTRLFRAME,
    QDISABLEFORCECONTROL,
    QSETENDFORCECOND
};

struct straight_traverse
{
    double dx, dy, dz;  // direction of original motion
    double x, y, z, a, b, c, u, v, w;
};

struct straight_feed
{
    double dx, dy, dz;                 // direction of original motion
    double x, y, z, a, b, c, u, v, w;  // target
};

struct arc_feed
{
    double original_turns;
    double end1, end2, center1, center2;
    int turn;
    double end3, a, b, c, u, v, w;
};

struct set_feed_rate
{
    double feed;
};

struct set_feed_mode
{
    int mode;
};

struct dwell
{
    double time;
};

struct ena_admit
{
    int enable;
};

struct comment
{
    char* comment;
};

struct mcommand
{
    int index;
    double p_number;
    double q_number;
};

struct orient_spindle
{
    double orientation;
    int mode;
};

struct wait_orient_spindle_complete
{
    double timeout;
};

struct settoolcommand
{
    double x_off;
    double y_off;
    double z_off;
    double a_off;
    double b_off;
    double c_off;
};

struct setuserframecommand
{
    double x_off;
    double y_off;
    double z_off;
    double a_off;
    double b_off;
    double c_off;
};

struct setpayloadcommand
{
    Payload payload;
};

struct setclsnlevelcommand
{
    int clsn_level;
};

struct setadmitctrconfigcommand
{
    int axis;
    int now;
    FtConfig ftConfig;  // force or torque control configuration
};

struct setcompliantconcommand
{
    double fx;
    double fy;
    double fz;
    double tx;
    double ty;
    double tz;
};

struct setcomplianttypecommand
{
    int compliantType;
    int compliantEnable;
};

struct setvelcompliantlevelcommand
{
    double opt;
    double rate1;
    double rate2;
    double rate3;
    double rate4;
};

struct setforcecontrolframecommand
{
    int ftFrame;
};

struct disableforcecontrolcommand
{
    int disableForceCtrl;
};

struct setendforcecondcommand
{
    int axis;
    EndForceCond endForceCond;  // force or torque control configuration
};

struct queued_canon
{
    int line_num;
    queued_canon_type type;
    short thread_id;
    union
    {
        struct straight_traverse straight_traverse;
        struct straight_feed straight_feed;
        struct arc_feed arc_feed;
        struct set_feed_rate set_feed_rate;
        struct dwell dwell;
        struct ena_admit enable_admit;
        struct set_feed_mode set_feed_mode;
        struct comment comment;
        struct mcommand mcommand;
        struct orient_spindle orient_spindle;
        struct wait_orient_spindle_complete wait_orient_spindle_complete;
        struct settoolcommand settool;
        struct setuserframecommand setuserframe;
        struct setpayloadcommand setpayload;
        struct setclsnlevelcommand setclsnlevel;
        struct setadmitctrconfigcommand setftconfig;
        struct setcompliantconcommand setcompliantcon;
        struct setcomplianttypecommand setcomplianttype;
        struct setvelcompliantlevelcommand setvelcompliantlevel;
        struct setforcecontrolframecommand setforcecontrolframe;
        struct disableforcecontrolcommand disableforcecontrol;
        struct setendforcecondcommand setendforcecond;
    } data;
};

std::vector<queued_canon>& qc(void);

void enqueue_ENABLEADMIT(int enable);
void enqueue_SET_FEED_RATE(double feed);
void enqueue_DWELL(int line_num, double time, short sub_thread);
void enqueue_ENABLEADMIT(int enable);
void enqueue_SET_FEED_MODE(int mode);
void enqueue_COMMENT(const char* c);
int enqueue_STRAIGHT_FEED(setup_pointer settings,
                          int l,
                          double dx,
                          double dy,
                          double dz,
                          double x,
                          double y,
                          double z,
                          double a,
                          double b,
                          double c,
                          double u,
                          double v,
                          double w);
int enqueue_STRAIGHT_TRAVERSE(setup_pointer settings,
                              int l,
                              double dx,
                              double dy,
                              double dz,
                              double x,
                              double y,
                              double z,
                              double a,
                              double b,
                              double c,
                              double u,
                              double v,
                              double w);
void enqueue_ARC_FEED(setup_pointer settings,
                      int l,
                      double original_arclen,
                      double end1,
                      double end2,
                      double center1,
                      double center2,
                      int turn,
                      double end3,
                      double a,
                      double b,
                      double c,
                      double u,
                      double v,
                      double w);
void enqueue_M_USER_COMMAND(int index, double p_number, double q_number);

void enqueue_SET_TOOL(int line_num, setup_pointer settings, double* toolOffset);
void enqueue_SET_USER_FRAME(int line_num, setup_pointer settings, double* userFrame);
void enqueue_SET_PAYLOAD(int line_num, setup_pointer settings, Payload payload);
void enqueue_SET_CLSN_LEVEL(int line_num, setup_pointer settings, int clsnLevel);
void enqueue_SET_FORCE_CTRL_FRAME(int line_num, setup_pointer settings, int ftFrame);
void enqueue_DISABLE_FORCE_CONTROL(int line_num, setup_pointer settings, int disableForceCtrl);
void enqueue_SET_ADMITCTRLCONFIG(int line_num, setup_pointer settings, int axis, FtConfig ftConfig, int now);
void enqueue_SET_ENDFORCECOND(int line_num, setup_pointer settings, int axis, EndForceCond endForceCond);
void enqueue_SET_COMPLIANT_CONDITION(int line_num, setup_pointer settings, double* compliantCondition);
void enqueue_SET_VEL_COMPLIANT_LEVEL(int line_num, setup_pointer settings, double* velCompliantCtrl);
void enqueue_SET_COMPLIANT_TYPE(int line_num, setup_pointer settings, int* typeEna);

void enqueue_SET_COMPLIANT_TYPE(int line_num, setup_pointer settings, int* typeEna);
void dequeue_canons(setup_pointer settings);
void set_endpoint(double x, double y);
void set_endpoint_zx(double z, double x);
int move_endpoint_and_flush(setup_pointer settings, double x, double y);
void qc_reset(void);
void qc_scale(double scale);
